#include "hex2int.h"

#define color_average_number 20

const unsigned char RED=0;
const unsigned char GREEN=1;
const unsigned char BLUE=2;
const unsigned char YELLOW=3;
unsigned char color_detected=-1;

// what does this do?
// how is this used?
unsigned long red_sum=0;
unsigned long green_sum=0;
unsigned long blue_sum=0;

/* <DEPRECATED_CODEBLOCK by=TimHaines when=05/17/2009> */
// I removed these functions so that no one will try to use them.

// Thank you, this read well and is understandable
//void jareds_read_color_sensor(void);
//void color_sensor_tester(void);
//
//void color_sensor_tester(void)
//{
//
////PC transmit goes to phone cable black
//	int temp;
//	//Just continously reads the color
//	//sensor and outputs to USART
//	
//	//usart_writeln("Color Sensor tester",1);
//	//usart_writeln("::",1);
//	//i
//	//v
//	//s
//	//c,0
//	//c,1
//	//c,2
//	//c,3
//	//d
//	usart_writeln("Color Sensor Tester",1);
//	usart_writeln("  ",1);
//	
//	
//	do
//	{
//		_delay_ms(100);
//		usart_write("!i",0);
//		usart_putchar(0x0D,0);
//		
//		_delay_ms(100);
//		usart_write("!v",0);
//		usart_putchar(0x0D,0);
//		
//		_delay_ms(100);
//		usart_write("!s",0);
//		usart_putchar(0x0D,0);		
//	
//	}while(usart_scan(0)!='!');
//	
//	writeln("Start Comms");
//	do
//	{
//		usart_write("!d",0);		//I guess 'initiate'
//		usart_putchar(0x0D,0);		//carriage return
//		temp = usart_scan(0);
//		
//		if (temp!=ERROR_CHAR)
//		{
//
//			usart_putchar(temp,1);
//			temp = usart_getchar(0);
//			do
//			{
//				usart_putchar(temp,1);
//				temp = usart_getchar(0);
//			
//			}while (temp!=0x0D);
//			
//			usart_putchar(temp,1);
//		}
//		_delay_ms(200);
//	}while( 1);	//wait for valid reply
//	
//	while(1)
//		usart_putchar(usart_getchar(0),1);
//	
//}
//
//
//void jareds_read_color_sensor(void)
//{
//	int temp, redtemp = 0, greentemp = 0, bluetemp = 0;
//	red_sum = blue_sum = green_sum = 0;
//	
//	for (int i = 0; i<color_average_number; i++)
//	{
//		do
//		{
//			usart_write("!i",0);		//I guess 'initiate'
//			usart_putchar(0x0d,0);		//carriage return
//		
//		}while( usart_scan(0) != 'd');	//wait for valid reply
//		
//		temp = usart_getchar(0);	//throw away comma
//		
//		//read in red data
//		redtemp = usart_getchar(0);		
//		redtemp = redtemp<<8 | usart_getchar(0);	
//		redtemp = redtemp<<8 | usart_getchar(0);
//		redtemp = redtemp<<8 | usart_getchar(0);
//
//		temp = usart_getchar(0);	//throw away comma
//		
//		//read in green data
//		greentemp = usart_getchar(0);		
//		greentemp = greentemp<<8 | usart_getchar(0);	
//		greentemp = greentemp<<8 | usart_getchar(0);
//		greentemp = greentemp<<8 | usart_getchar(0);
//		
//		usart_getchar(0); // ignore comma
//		
//		//read in blue data
//		bluetemp = usart_getchar(0);		
//		bluetemp = bluetemp<<8 | usart_getchar(0);	
//		bluetemp = bluetemp<<8 | usart_getchar(0);
//		bluetemp = bluetemp<<8 | usart_getchar(0);
//	
//		red_sum+=redtemp;
//		blue_sum+=bluetemp;
//		green_sum+=greentemp;
//	
//	}
//	
//	red_sum 	/=	color_average_number;
//	blue_sum 	/=	color_average_number;
//	green_sum	/=	color_average_number;
//	
//}
/* </DEPRECATED_CODEBLOCK> */


int read_color_sensor(void)
{
	
	int i;
	unsigned char redtemp[4];
	unsigned char greentemp[4];
	unsigned char bluetemp[4];
	unsigned char tmp;
	//long redsum=0;
	//long greensum=0;
	//long bluesum=0;
	//int green_red=0;
	//int blue_red=0;
	//int blue_green=0;


	//usart_writeln("Color Sensor Tester",1);
	//usart_writeln("  ",1);
	
	
	do
	{
		_delay_ms(100);
		usart_write("!i",0);
		usart_putchar(0x0D,0);
		
		//_delay_ms(100);
		//usart_write("!v",0);
		//usart_putchar(0x0D,0);
		
		//_delay_ms(100);
		//usart_write("!s",0);
		//usart_putchar(0x0D,0);
	
	}while(usart_scan(0)!='!');
	
	//writeln("Start Comms");
		
	//reads and averages color sensor values
	
	
	for(i=0; i<color_average_number; i++)
	{
		while( usart_scan(0) != 'd' )
		{
			usart_write("!d",0);   // output !i
			usart_putchar(0x0D,0);   // output 0d carrage return
			
			//usart_putchar(0x0,0);   // output 0a line feed
			//uasrt_putchar(0xa,0);
		}
	

		//while(usart_getchar(0)==','); // ignore comma
		do{
			tmp = usart_getchar(0);
		}while(tmp == ',');
		
		//redtemp[0] = usart_getchar(0);
		redtemp[0] = tmp;
		redtemp[1] = usart_getchar(0);
		redtemp[2] = usart_getchar(0);
		redtemp[3] = usart_getchar(0);
		
		usart_putchar(redtemp[0],0);
		usart_putchar(redtemp[1],0);
		usart_putchar(redtemp[2],0);
		usart_putchar(redtemp[3],0);
		
		
		//while(usart_getchar(0)==',');
		do{
			tmp = usart_getchar(0);
		}while(tmp == ',');
		
		//greentemp[0] = usart_getchar(0);
		greentemp[0] = tmp;
		greentemp[1] = usart_getchar(0);
		greentemp[2] = usart_getchar(0);
		greentemp[3] = usart_getchar(0);
		
		usart_putchar(' ',0);
		usart_putchar(greentemp[0],0);
		usart_putchar(greentemp[1],0);
		usart_putchar(greentemp[2],0);
		usart_putchar(greentemp[3],0);
		
		//while(usart_getchar(0)==',');
		do{
			tmp = usart_getchar(0);
		}while(tmp == ',');
		
		//bluetemp[0] = usart_getchar(0);
		usart_putchar(0x0D,0);		
		bluetemp[0] = tmp;
		bluetemp[1] = usart_getchar(0);
		bluetemp[2] = usart_getchar(0);
		bluetemp[3] = usart_getchar(0);
		
		usart_putchar(bluetemp[0],0);
		usart_putchar(bluetemp[1],0);
		usart_putchar(bluetemp[2],0);
		usart_putchar(bluetemp[3],0);
		
		//usart_getchar(0); // ignore comma
		
		//usart_getchar(0); // ignore recognized color
		//usart_getchar(0); // ignore recognized color

		//redsum += htoi(redtemp);
		//greensum += htoi(greentemp);
		//bluesum += htoi(bluetemp);
		
	}
	
		
		//green_red = 1000*(greensum/color_average_number)/(redsum/color_average_number);
		//blue_red = 1000*(bluesum/color_average_number)/(redsum/color_average_number);
		//blue_green = 1000*(bluesum/color_average_number)/(greensum/color_average_number);
		//
		//if(green_red < 200 &&  blue_red < 200 && blue_green > 850 )
		//{
		//	writeln("Detect Red");
		//	return	RED;
		//}
		//
		//if(green_red < 1250 &&  blue_red > 850 && blue_red < 1250 && blue_green < 1000 )
		//{
		//	writeln("Detect Green");
		//	return	GREEN;
		//}
		//
		//if(green_red > 1350 &&  blue_red > 2600  && blue_green > 1500 )
		//{
		//	writeln("Detect Blue");
		//	return BLUE;
		//}
		//
		//if(green_red < 1020 &&  blue_red < 1020 && blue_green < 1020 && green_red > 650 &&  blue_red > 160 && blue_green > 240 )
		//{
		//	writeln("Detect Yellow");
		//	return	YELLOW;
		//}
		//
		//writeln("No Box");
		return -1;
}